//
// Created by Ivor_ on 2022/3/12.
//

#include "interpolation.h"

double* lagrangeIp(double** points, int numOfPoints, double* objPoints, int numOfObjPoints) {
    double* objVals = (double*)calloc(numOfObjPoints, sizeof(double));
    for (int i = 0; i < numOfObjPoints; ++i) {
        objVals[i] = 0.;
        for (int j = 0; j < numOfPoints; ++j) {
            double temp = 1.;
            for (int k = 0; k < numOfPoints; ++k) {
                if (j != k) {
                    temp *= (objPoints[i]-points[k][0]) / (points[j][0]-points[k][0]);
                }
            }
            objVals[i] += (temp*points[j][1]);
        }
    }
    return objVals;
}

double* newtonIp(double** points, int numOfPoints, double* objPoints, int numOfObjPoints) {
    double* diff = (double*)calloc(numOfPoints, sizeof(double));
    for (int i = 0; i < numOfPoints; ++i) {
        diff[i] = points[i][1];
    }
    for (int i = 1; i < numOfPoints; ++i) {
        for (int j = numOfPoints-1; j >= i; --j) {
            diff[j] = (diff[j]-diff[j-1]) / (points[j][0]-points[j-i][0]);
        }
    }
    double* objVals = (double*)calloc(numOfObjPoints, sizeof(double));
    for (int i = 0; i < numOfObjPoints; ++i) {
        double temp = 1.;
        objVals[i] = points[0][1];
        for (int j = 1; j < numOfPoints; ++j) {
            temp *= (objPoints[i] - points[j-1][0]);
            objVals[i] += (temp*diff[j]);
        }
    }
    free(diff);
    return objVals;
}

double* hermiteIp(double** points, int numOfPoints, double* objPoints, int numOfObjPoints) {
    double* objVals = (double*)calloc(numOfObjPoints, sizeof(double));
    for (int i = 0; i < numOfObjPoints; ++i) {
        objVals[i] = 0.;
        for (int j = 0; j < numOfPoints; ++j) {
            double ll = 1.;
            for (int k = 0; k < numOfPoints; ++k) {
                if (j != k) {
                    ll *= (objPoints[i]-points[k][0]) / (points[j][0]-points[k][0]);
                }
            }
            double hbh = ll*ll, hbg = (objPoints[i]-points[j][0]) * ll * ll;
            for (int k = 0; k < numOfPoints; ++k) {
                if (j != k) {
                    hbh -= 2. * (objPoints[i]-points[j][0]) / (points[j][0]-points[k][0]) * ll * ll;
                }
            }
            objVals[i] += (hbh*points[j][1] + hbg*points[j][2]);
        }
    }
    return objVals;
}

double* sectionIp(double** points, int numOfPoints, double* objPoints, int numOfObjPoints) {
    double* objVals = (double*)calloc(numOfObjPoints, sizeof(double));
    for (int i = 0; i < numOfObjPoints; ++i) {
        int i1 = -1, i2 = -1;
        for (int j = 0; j < numOfPoints; ++j) {
            if (points[j][0] < objPoints[i]) {
                if (i1 < 0 || points[i1][0] < points[j][0]) {
                    i1 = j;
                }
            } else {
                if (i2 < 0 || points[i2][0] > points[j][0]) {
                    i2 = j;
                }
            }
        }
        objVals[i] = (objPoints[i]-points[i2][0]) / (points[i1][0]-points[i2][0]) * points[i1][1];
        objVals[i] += (objPoints[i]-points[i1][0]) / (points[i2][0]-points[i1][0]) * points[i2][1];
    }
    return objVals;
}

double* cubicSplineIp(double** points, int numOfPoints, char method, char boundary, double bs, double be, double* objPoints, int numOfObjPoints) {
    numOfPoints--;
    int ll = numOfPoints, rr = numOfPoints;
    double** matrix;
    if (method == 'M' && boundary == 'M') {
        ll--;
        matrix = (double**)calloc(ll, sizeof(double*));
        for (int i = 0; i < ll; ++i) {
            matrix[i] = (double*)calloc(rr, sizeof(double));
            for (int j = 0; j < rr; ++j) {
                matrix[i][j] = 2. * (i == j);
            }
        }
        for (int i = 0; i < ll-1; ++i) {
            matrix[i][i+1] = (points[i+2][0]-points[i+1][0]) / (points[i+2][0]-points[i][0]);
            matrix[i+1][i] = (points[i+2][0]-points[i+1][0]) / (points[i+3][0]-points[i+1][0]);
            matrix[i][rr-1] = 6. * (((points[i+2][1] - points[i+1][1]) / (points[i+2][0] - points[i+1][0])) - ((points[i+1][1] - points[i][1]) / (points[i+1][0] - points[i][0]))) / (points[i+2][0] - points[i][0]);
        }
        matrix[ll-1][rr-1] = 6. * (((points[ll+1][1] - points[ll][1]) / (points[ll+1][0] - points[ll][0])) - ((points[ll][1] - points[ll-1][1]) / (points[ll][0] - points[ll-1][0]))) / (points[ll+1][0] - points[ll-1][0]);
        matrix[0][rr-1] -= (points[1][0]-points[0][0]) / (points[2][0]-points[0][0]) * bs;
        matrix[ll-1][rr-1] -= (points[ll+1][0]-points[ll][0]) / (points[ll+1][0]-points[ll-1][0]) * be;
    } else if (method == 'm' && boundary == 'm') {
        ll--;
        matrix = (double**)calloc(ll, sizeof(double*));
        for (int i = 0; i < ll; ++i) {
            matrix[i] = (double*)calloc(rr, sizeof(double));
            for (int j = 0; j < rr; ++j) {
                matrix[i][j] = 2. * (i == j);
            }
        }
        for (int i = 0; i < ll-1; ++i) {
            matrix[i][i+1] = (points[i+1][0]-points[i][0]) / (points[i+2][0]-points[i][0]);
            matrix[i+1][i] = (points[i+3][0]-points[i+2][0]) / (points[i+3][0]-points[i+1][0]);
            matrix[i][rr-1] = 3. * ((points[i+2][0] - points[i+1][0])/(points[i+2][0] - points[i][0])*(points[i+1][1] - points[i][1])/(points[i+1][0] - points[i][0])
                    + (points[i+1][0] - points[i][0])/(points[i+2][0] - points[i][0])*(points[i+2][1] - points[i+1][1])/(points[i+2][0] - points[i+1][0]));
        }
        matrix[ll-1][rr-1] = 3. * ((points[ll+1][0] - points[ll][0])/(points[ll+1][0] - points[ll-1][0])*(points[ll][1] - points[ll-1][1])/(points[ll][0] - points[ll-1][0])
                + (points[ll][0] - points[ll-1][0])/(points[ll+1][0] - points[ll-1][0])*(points[ll+1][1] - points[ll][1])/(points[ll+1][0] - points[ll][0]));
        matrix[0][rr-1] -= (points[2][0]-points[1][0]) / (points[2][0]-points[0][0]) * bs;
        matrix[ll-1][rr-1] -= (points[ll][0]-points[ll-1][0]) / (points[ll+1][0]-points[ll-1][0]) * be;
    } else if (method == 'M' && boundary == 'm') {
        ll++;
        rr += 2;
        matrix = (double**)calloc(ll, sizeof(double*));
        for (int i = 0; i < ll; ++i) {
            matrix[i] = (double*)calloc(rr, sizeof(double));
            for (int j = 0; j < rr; ++j) {
                matrix[i][j] = 2. * (i == j);
            }
        }
        for (int i = 1; i < ll-1; ++i) {
            matrix[i][i-1] = (points[i][0] - points[i-1][0]) / (points[i+1][0] - points[i-1][0]);
            matrix[i][i+1] = (points[i+1][0] - points[i][0]) / (points[i+1][0] - points[i-1][0]);
            matrix[i][rr-1] = 6. * (((points[i+1][1] - points[i][1]) / (points[i+1][0] - points[i][0])) - ((points[i][1] - points[i-1][1]) / (points[i][0] - points[i-1][0]))) / (points[i+1][0] - points[i-1][0]);
        }
        matrix[0][0] = 2.;
        matrix[0][1] = 1.;
        matrix[0][rr-1] = 6. / (points[1][0]-points[0][0]) * (((points[1][1] - points[0][1])/(points[1][0] - points[0][0])) - bs);
        matrix[ll-1][rr-3] = 1.;
        matrix[ll-1][rr-2] = 2.;
        matrix[ll-1][rr-1] = 6. / (points[ll-1][0]-points[ll-2][0]) * (be - ((points[ll-1][1] - points[ll-2][1])/(points[ll-1][0] - points[ll-2][0])));
    } else if (method == 'm' && boundary == 'M') {
        ll++;
        rr += 2;
        matrix = (double**)calloc(ll, sizeof(double*));
        for (int i = 0; i < ll; ++i) {
            matrix[i] = (double*)calloc(rr, sizeof(double));
            for (int j = 0; j < rr; ++j) {
                matrix[i][j] = 2. * (i == j);
            }
        }
        for (int i = 1; i < ll-1; ++i) {
            matrix[i][i-1] = (points[i+1][0]-points[i][0]) / (points[i+1][0]-points[i-1][0]);
            matrix[i][i+1] = (points[i][0]-points[i-1][0]) / (points[i+1][0]-points[i-1][0]);
            matrix[i][rr-1] = 3. * ((points[i+1][0]-points[i][0]) / (points[i+1][0]-points[i-1][0]) * (points[i][1]-points[i-1][1]) / (points[i][0]-points[i-1][0])
                    + (points[i][0]-points[i-1][0]) / (points[i+1][0]-points[i-1][0]) * (points[i+1][1]-points[i][1]) / (points[i+1][0]-points[i][0]));
        }
        matrix[0][0] = 2.;
        matrix[0][1] = 1.;
        matrix[0][rr-1] = 3. * ((points[0][0]-points[1][0]) / 6. * bs + (points[1][1]-points[0][1]) / (points[1][0]-points[0][0]));
        matrix[ll-1][rr-3] = 1.;
        matrix[ll-1][rr-2] = 2.;
        matrix[ll-1][rr-1] = 3. * ((points[ll-1][1]-points[ll-2][1]) / (points[ll-1][0]-points[ll-2][0]) + (points[ll-1][0]-points[ll-2][0]) / 6. * be);
    } else {
        fprintf(stderr, "WRONG ARGUMENT!\n");
        exit(-1);
    }
    double** matrix0 = dMatrixLineSimBeta(simDim(2, ll, rr), matrix);
    freeDMat(ll, matrix);
    matrix = matrix0;
    double* objVals = (double*)calloc(numOfObjPoints, sizeof(double));
    for (int i = 0; i < numOfObjPoints; ++i) {
        int i1 = -1, i2 = -1;
        for (int j = 0; j <= numOfPoints; ++j) {
            if (points[j][0] < objPoints[i]) {
                if (i1 < 0 || points[i1][0] < points[j][0]) {
                    i1 = j;
                }
            } else {
                if (i2 < 0 || points[i2][0] > points[j][0]) {
                    i2 = j;
                }
            }
        }
        if (method == 'M') {
            if (method == boundary) {
                if (i1 == 0) {
                    objVals[i] = (pow((points[i2][0] - objPoints[i]), 3.)*bs + pow((objPoints[i] - points[i1][0]), 3.)*matrix[i2-1][rr-1]) / 6. / (points[i2][0]-points[i1][0])
                            + ((points[i2][0] - objPoints[i])*points[i1][1] + (objPoints[i] - points[i1][0])*points[i2][1]) / (points[i2][0]-points[i1][0]);
                    objVals[i] -= (points[i2][0]-points[i1][0]) / 6. * ((points[i2][0] - objPoints[i])*bs + (objPoints[i] - points[i1][0])*matrix[i2-1][rr-1]);
                } else if (i2 == numOfPoints) {
                    objVals[i] = (pow((points[i2][0] - objPoints[i]), 3.)*matrix[i1-1][rr-1] + pow((objPoints[i] - points[i1][0]), 3.)*be) / 6. / (points[i2][0]-points[i1][0])
                                 + ((points[i2][0] - objPoints[i])*points[i1][1] + (objPoints[i] - points[i1][0])*points[i2][1]) / (points[i2][0]-points[i1][0]);
                    objVals[i] -= (points[i2][0]-points[i1][0]) / 6. * ((points[i2][0] - objPoints[i])*matrix[i1-1][rr-1] + (objPoints[i] - points[i1][0])*be);
                } else {
                    objVals[i] = (pow((points[i2][0] - objPoints[i]), 3.)*matrix[i1-1][rr-1] + pow((objPoints[i] - points[i1][0]), 3.)*matrix[i2-1][rr-1]) / 6. / (points[i2][0]-points[i1][0])
                            + ((points[i2][0] - objPoints[i])*points[i1][1] + (objPoints[i] - points[i1][0])*points[i2][1]) / (points[i2][0]-points[i1][0]);
                    objVals[i] -= (points[i2][0]-points[i1][0]) / 6. * ((points[i2][0] - objPoints[i])*matrix[i1-1][rr-1] + (objPoints[i] - points[i1][0])*matrix[i2-1][rr-1]);
                }
            } else {
                objVals[i] = (pow((points[i2][0] - objPoints[i]), 3.)*matrix[i1][rr-1] + pow((objPoints[i] - points[i1][0]), 3.)*matrix[i2][rr-1]) / 6. / (points[i2][0]-points[i1][0])
                        + ((points[i2][0] - objPoints[i])*points[i1][1] + (objPoints[i] - points[i1][0])*points[i2][1]) / (points[i2][0]-points[i1][0]);
                objVals[i] -= (points[i2][0]-points[i1][0]) / 6. * ((points[i2][0] - objPoints[i])*matrix[i1][rr-1] + (objPoints[i] - points[i1][0])*matrix[i2][rr-1]);
            }
        } else {
            if (method == boundary) {
                if (i1 == 0) {
                    objVals[i] = (1. + 2.*(objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * points[i1][1]
                            + (objPoints[i]-points[i1][0]) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * bs;
                    objVals[i] += (1. + 2.*(objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * points[i2][1]
                            + (objPoints[i]-points[i2][0]) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * matrix[i2-1][rr-1];
                } else if (i2 == numOfPoints) {
                    objVals[i] = (1. + 2.*(objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * points[i1][1]
                            + (objPoints[i]-points[i1][0]) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * matrix[i1-1][rr-1];
                    objVals[i] += (1. + 2.*(objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * points[i2][1]
                            + (objPoints[i]-points[i2][0]) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * be;
                } else {
                    objVals[i] = (1. + 2.*(objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * points[i1][1]
                            + (objPoints[i]-points[i1][0]) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * matrix[i1-1][rr-1];
                    objVals[i] += (1. + 2.*(objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * points[i2][1]
                            + (objPoints[i]-points[i2][0]) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * matrix[i2-1][rr-1];
                }
            } else {
                objVals[i] = (1. + 2.*(objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * points[i1][1]
                        + (objPoints[i]-points[i1][0]) * pow(((objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])), 2.) * matrix[i1][rr-1];
                objVals[i] += (1. + 2.*(objPoints[i] - points[i2][0])/(points[i1][0] - points[i2][0])) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * points[i2][1]
                        + (objPoints[i]-points[i2][0]) * pow(((objPoints[i] - points[i1][0])/(points[i2][0] - points[i1][0])), 2.) * matrix[i2][rr-1];
            }
        }
    }
    freeDMat(ll, matrix);
    return objVals;
}
